//Servo command using a Pololu Micro Maestro 6-channel USB Servo Controller
//targets are the servo position in us
void write4servos(unsigned int target1, unsigned int target2, unsigned int target3, unsigned int target4){
  byte buff[16];
  buff[0] = 0x84; //start byte
  buff[1] = 0; //servo number
  buff[2] = (target1 << 2) & 0x7F; // Second byte holds the lower 7 bits of target.  
  buff[3] = (target1 >> 5) & 0x7F; // Third data byte holds the bits 7-13 of target.

  buff[4] = 0x84; //start byte
  buff[5] = 1; //servo number
  buff[6] = (target2 << 2) & 0x7F; // Second byte holds the lower 7 bits of target.  
  buff[7] = (target2 >> 5) & 0x7F; // Third data byte holds the bits 7-13 of target.

  buff[8] = 0x84; //start byte
  buff[9] = 2; //servo number
  buff[10] = (target3 << 2) & 0x7F; // Second byte holds the lower 7 bits of target.  
  buff[11] = (target3 >> 5) & 0x7F; // Third data byte holds the bits 7-13 of target.

  buff[12] = 0x84; //start byte
  buff[13] = 3; //servo number
  buff[14] = (target4 << 2) & 0x7F; // Second byte holds the lower 7 bits of target.  
  buff[15] = (target4 >> 5) & 0x7F; // Third data byte holds the bits 7-13 of target.

  SerialPort.write(buff, 16); 
}


//Servo command using a Pololu Micro Maestro 6-channel USB Servo Controller
//servo is the servo number (0-5)
//target is the servo position in us
void write1servos(byte servo, unsigned int target){
  byte buff[4];

  buff[0] = 0x84; //start byte
  buff[1] = servo; //servo number
  buff[2] = (target << 2) & 0x7F; // Second byte holds the lower 7 bits of target.  
  buff[3] = (target >> 5) & 0x7F; // Third data byte holds the bits 7-13 of target.

  SerialPort.write(buff, 4);
}


